#include <PS2X_lib.h>
#include <Servo.h>

PS2X ps2x; // create PS2 Controller Class

Servo myservo;

int pos;

#define gpio0 3  //舵机的信号引脚
#define gpio1 4  //小车的四个引脚
#define gpio2 2  
#define gpio3 8
#define gpio4 7  
#define en1 5     //L298N的控速引脚
#define en2 6    

#define v1 225  //前进后退速度
#define v2 180   //左转右转速度


int error = 0; 
byte type = 0;
byte vibrate = 0;

void qianjin(void){
      Serial.print("Got command: ");  
      Serial.println("前进");

      analogWrite(en1,v1); 
      analogWrite(en2,v1);
      digitalWrite(gpio1, HIGH);  
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, HIGH);  
      digitalWrite(gpio4, LOW);
      
}
void houtui(void){
      Serial.print("Got command: ");  
      Serial.println("后退");

      analogWrite(en1,v1); 
      analogWrite(en2,v1);
      
      digitalWrite(gpio1, LOW);  
      digitalWrite(gpio2, HIGH);
      digitalWrite(gpio3, LOW);  
      digitalWrite(gpio4, HIGH);
       
}
void zuozhuan(void){
      Serial.print("Got command: ");  
      Serial.println("左转");

      analogWrite(en1,v2); 
      analogWrite(en2,v2);
      digitalWrite(gpio1, HIGH);  
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, LOW);  
      digitalWrite(gpio4, HIGH);
        
}
void youzhuan(void){
      Serial.print("Got command: ");  
      Serial.println("右转");

      analogWrite(en1,v2); 
      analogWrite(en2,v2);
      digitalWrite(gpio1, LOW);  
      digitalWrite(gpio2, HIGH);
      digitalWrite(gpio3, HIGH);  
      digitalWrite(gpio4, LOW);
       
}
void tingzhi(void){
      analogWrite(en1,0); 
      analogWrite(en2,0);
      digitalWrite(gpio1, LOW);  
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, LOW);  
      digitalWrite(gpio4, LOW);     
}
void bihe(void){
      for (pos = 0; pos <= 90; pos ++) { // 0°到180°
        myservo.write(pos);              // 舵机角度写入
        delay(5);                       // 等待转动到指定角度
      }
      Serial.println("Got command:舵机关闭");
}
void dakai(void){
      for (pos = 90; pos >= 0; pos --) { // 从180°到0°
        myservo.write(pos);              // 舵机角度写入
        delay(5);                       // 等待转动到指定角度
      }
      Serial.println("Got command:舵机打开");
}
void chushihua(void){
      pinMode(gpio1, OUTPUT); 
      pinMode(gpio2, OUTPUT);
      pinMode(gpio3, OUTPUT);
      pinMode(gpio4, OUTPUT); 
   
      digitalWrite(gpio1, LOW);
      digitalWrite(gpio2, LOW);
      digitalWrite(gpio3, LOW);
      digitalWrite(gpio4, LOW);
}


void setup(){
  Serial.begin(9600);
  chushihua();
  myservo.attach(gpio0);  
  myservo.write(45);
  //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
  
  error = ps2x.config_gamepad(13,11,10,12, true, true);   //setup pins and settings:  GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error

if(error == 0){
   Serial.println("Found Controller, configured successful");
   Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
  Serial.println("holding L1 or R1 will print out the analog stick values.");
  Serial.println("Go to [url]www.billporter.info[/url] for updates and to report bugs.");
  }
  else if(error == 1)
   Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit [url]www.billporter.info[/url] for troubleshooting tips");
   
  else if(error == 2)
   Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit [url]www.billporter.info[/url] for troubleshooting tips");
   
  else if(error == 3)
   Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
   
   type = ps2x.readType(); 
     switch(type) {
       case 0:
        Serial.println("Unknown Controller type");
       break;
       case 1:
        Serial.println("DualShock Controller Found");
       break;
       case 2:
         Serial.println("GuitarHero Controller Found");
       break;
     }
  
}

void loop(){
   
  
    
  if(type == 1||type == 0){//Guitar Hero Controller
     ps2x.read_gamepad(false, vibrate);          //read controller and set large motor to spin at 'vibrate' speed
     vibrate = ps2x.Analog(PSAB_BLUE); 
     if(ps2x.Button(PSB_START))                   //will be TRUE as long as button is pressed
         Serial.println("Start is being held");
     else if(ps2x.Button(PSB_SELECT))
         Serial.println("Select is being held");   
      else if(ps2x.Button(PSB_PAD_UP)) {         //前进
       qianjin();
      }
      else if(ps2x.Button(PSB_PAD_RIGHT)){    //右转
       youzhuan();
      }
      else if(ps2x.Button(PSB_PAD_LEFT)){    //左转
       zuozhuan();
      }
      else if(ps2x.Button(PSB_PAD_DOWN)){    //后退
       houtui();
      }   
      else if(ps2x.Button(PSB_L1)){
        bihe();
      }
      else if(ps2x.Button(PSB_R1)){
        dakai();
      }      
      else tingzhi();
  }
  delay(50);
}
